
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_baro.c
  * @author     baiyang
  * @date       2021-11-21
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_baro.h"

#include "sensor_baro_ms5611.h"
#include "sensor_baro_spl06.h"

#include <stdio.h>
#include <float.h>
#include <rtconfig.h>

#include <parameter/param.h>
#include <gcs_mavlink/gcs.h>
#include <device_manager/dev_mgr.h>
#include <device_manager/dev_mgr_i2c.h>
#include <common/console/console.h>
#include <board_config/borad_config.h>
#include <common/gp_math/gp_mathlib.h>
#include <common/time/gp_time.h>
#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <notify/notify.h>
/*-----------------------------------macro------------------------------------*/
#define SENSOR_BARO_GCS_SEND_TEXT(severity, format, ...) gcs_send_text(severity, format, ##__VA_ARGS__)

#define INTERNAL_TEMPERATURE_CLAMP 35.0f

#ifndef HAL_BARO_FILTER_DEFAULT
 #define HAL_BARO_FILTER_DEFAULT 0 // turned off by default
#endif

#if !defined(HAL_PROBE_EXTERNAL_I2C_BAROS) && !defined(HAL_MINIMIZE_FEATURES)
#define HAL_PROBE_EXTERNlAL_I2C_BAROS
#endif

#ifndef HAL_BARO_PROBE_EXT_DEFAULT
 #define HAL_BARO_PROBE_EXT_DEFAULT 0
#endif

#ifndef HAL_BARO_EXTERNAL_BUS_DEFAULT
 #define HAL_BARO_EXTERNAL_BUS_DEFAULT -1
#endif

#ifdef HAL_BUILD_AP_PERIPH
#define HAL_BARO_ALLOW_INIT_NO_BARO
#endif

/*
  macro to add a backend with check for too many sensors
 We don't try to start more than the maximum allowed
 */
#define ADD_BACKEND(backend) \
    do { _add_backend(backend);     \
       if (_sensor_baro._num_drivers == BARO_MAX_DRIVERS || \
          _sensor_baro._num_sensors == BARO_MAX_INSTANCES) { \
          return; \
       } \
    } while (0)

// macro for use by HAL_INS_PROBE_LIST
#define GET_I2C_DEVICE(bus, address) devmgr_get_i2c_device(bus, address)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_baro_publish_baro(void);
static bool _add_backend(sensor_baro_backend *backend);
static void _probe_i2c_barometers(void);
/*----------------------------------variable----------------------------------*/
static sensor_baro _sensor_baro;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
 * Get the sensor_baro singleton
 */
sensor_baro *sensor_baro_get_singleton()
{
    return &_sensor_baro;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_baro_ctor()
{
    _sensor_baro._log_baro_bit = -1;

    derivative_filter7(&_sensor_baro._climb_rate_filter);

    sensor_baro_assign_param();
}

/*
  initialise the barometer object, loading backend drivers
 */
void sensor_baro_init(void)
{
    _sensor_baro.init_done = true;

    // ensure that there isn't a previous ground temperature saved
    if (!math_flt_zero(_sensor_baro._user_ground_temperature)) {
        param_set_and_save(PARAM_ID(BARO, BARO_GND_TEMP), 0.0f);
        //_sensor_baro._user_ground_temperature.notify();
    }

    // zero bus IDs before probing
    for (uint8_t i = 0; i < BARO_MAX_INSTANCES; i++) {
        _sensor_baro.sensors[i].bus_id = 0;
    }

    switch (brd_get_board_type()) {
    case PX4_BOARD_PIXHAWK:
    case PX4_BOARD_PHMINI:
    case PX4_BOARD_AUAV21:
    case PX4_BOARD_PH2SLIM:
    case PX4_BOARD_PIXHAWK_PRO:
    case PX4_BOARD_FMUV5:
    case PX4_BOARD_FMUV6:
    case PX4_BOARD_AP0T:
        ADD_BACKEND(sensor_baro_ms56xx_probe(devmgr_get_spi_device(MS5611_SPI_DEVICE_NAME), BARO_MS5611));
        break;
    case PX4_BOARD_APMCNV1:
        FOREACH_I2C_EXTERNAL(i) {
            ADD_BACKEND(sensor_baro_spl06_probe(GET_I2C_DEVICE(i, HAL_BARO_SPL06_I2C_ADDR)));
        }

        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(sensor_baro_spl06_probe(GET_I2C_DEVICE(i, HAL_BARO_SPL06_I2C_ADDR)));
        }
        break;
    case PX4_BOARD_APGDSTV1:
        FOREACH_I2C_INTERNAL(i) {
            ADD_BACKEND(sensor_baro_spl06_probe(GET_I2C_DEVICE(i, HAL_BARO_SPL06_I2C_ADDR)));
        }

        ADD_BACKEND(sensor_baro_ms56xx_probe(devmgr_get_spi_device(MS5611_SPI_DEVICE_NAME), BARO_MS5611));
        break;
    default:
        break;
    }

    // can optionally have baro on I2C too
    if (_sensor_baro._ext_bus >= 0) {
        ADD_BACKEND(sensor_baro_ms56xx_probe(GET_I2C_DEVICE(_sensor_baro._ext_bus, HAL_BARO_MS5611_I2C_ADDR), BARO_MS5611));
    }

#ifdef HAL_PROBE_EXTERNAL_I2C_BAROS
    _probe_i2c_barometers();
#endif

#if !defined(HAL_BARO_ALLOW_INIT_NO_BARO) // most boards requires external baro
    if (_sensor_baro._num_drivers == 0 || _sensor_baro._num_sensors == 0 || _sensor_baro.drivers[0] == NULL) {
        brd_config_error("Baro: unable to initialise driver");
    }
#endif
}

// healthy - returns true if sensor and derived altitude are good
bool sensor_baro_healthy()
{
    return sensor_baro_healthy2(_sensor_baro._primary);
}

bool sensor_baro_healthy2(uint8_t instance)
{
    return _sensor_baro.sensors[instance].healthy && _sensor_baro.sensors[instance].alt_ok && _sensor_baro.sensors[instance].calibrated;
}

// get current altitude in meters relative to altitude at the time
// of the last calibrate() call
float sensor_baro_get_altitude(void)
{
    return sensor_baro_get_altitude2(_sensor_baro._primary);
}
float sensor_baro_get_altitude2(uint8_t instance)
{
    return _sensor_baro.sensors[instance].altitude;
}

// get current altitude in meters above sea level
float sensor_baro_get_altitude_abs(void)
{
    return _sensor_baro.sensors[_sensor_baro._primary].altitude_abs;
}
float sensor_baro_get_altitude_abs2(uint8_t instance)
{
    return _sensor_baro.sensors[instance].altitude_abs;
}

// get last time sample was taken (in ms)
uint32_t sensor_baro_get_last_update(void)
{
    return sensor_baro_get_last_update2(_sensor_baro._primary);
}
uint32_t sensor_baro_get_last_update2(uint8_t instance)
{
    return _sensor_baro.sensors[instance].last_update_ms;
}

// get last time sample was taken (in us)
uint64_t sensor_baro_get_last_update_usec(void)
{
    return sensor_baro_get_last_update2_usec(_sensor_baro._primary);
}
uint64_t sensor_baro_get_last_update2_usec(uint8_t instance)
{
    return _sensor_baro.sensors[instance].last_update_us;
}

// returns the ground temperature in degrees C, selecting either a user
// provided one, or the internal estimate
float sensor_baro_get_ground_temperature(void)
{
    if (math_flt_zero(_sensor_baro._user_ground_temperature)) {
        return _sensor_baro._guessed_ground_temperature;
    } else {
        return _sensor_baro._user_ground_temperature;
    }
}

// pressure in Pascal. Divide by 100 for millibars or hectopascals
float sensor_baro_get_pressure(void) { return sensor_baro_get_pressure2(_sensor_baro._primary); }
float sensor_baro_get_pressure2(uint8_t instance) { return _sensor_baro.sensors[instance].pressure; }

// temperature in degrees C
float sensor_baro_get_temperature(void) { return sensor_baro_get_temperature2(_sensor_baro._primary); }
float sensor_baro_get_temperature2(uint8_t instance) { return _sensor_baro.sensors[instance].temperature; }

float sensor_baro_get_external_temperature(void) { return sensor_baro_get_external_temperature2(_sensor_baro._primary); };

// return altitude difference in meters between current pressure and a
// given base_pressure in Pascal
float sensor_baro_get_altitude_difference(float base_pressure, float pressure)
{
    float ret;
    float temp    = sensor_baro_get_ground_temperature() + C_TO_KELVIN;
    float scaling = pressure / base_pressure;

    // This is an exact calculation that is within +-2.5m of the standard
    // atmosphere tables in the troposphere (up to 11,000 m amsl).
    ret = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)));

    return ret;
}

/*
  call update on all drivers
 */
void sensor_baro_update(void)
{
    if (fabsf(_sensor_baro._alt_offset - _sensor_baro._alt_offset_active) > 0.01f) {
        // If there's more than 1cm difference then slowly slew to it via LPF.
        // The EKF does not like step inputs so this keeps it happy.
        _sensor_baro._alt_offset_active = (0.95f*_sensor_baro._alt_offset_active) + (0.05f*_sensor_baro._alt_offset);
    } else {
        _sensor_baro._alt_offset_active = _sensor_baro._alt_offset;
    }

    for (uint8_t i=0; i<_sensor_baro._num_drivers; i++) {
        sensor_baro_backend_update(_sensor_baro.drivers[i], i);
    }

    for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
        if (_sensor_baro.sensors[i].healthy) {
            // update altitude calculation
            float ground_pressure = _sensor_baro.sensors[i].ground_pressure;
            if (!math_flt_positive(ground_pressure) || isnan(ground_pressure) || isinf(ground_pressure)) {
                _sensor_baro.sensors[i].ground_pressure = _sensor_baro.sensors[i].pressure;
            }
            float altitude = _sensor_baro.sensors[i].altitude;
            float altitude_abs = _sensor_baro.sensors[i].altitude_abs;
            float corrected_pressure = _sensor_baro.sensors[i].pressure + _sensor_baro.sensors[i].p_correction;
            if (_sensor_baro.sensors[i].type == BARO_TYPE_AIR) {
                altitude = sensor_baro_get_altitude_difference(_sensor_baro.sensors[i].ground_pressure, corrected_pressure);
                altitude_abs = sensor_baro_get_altitude_difference(101325.0f, corrected_pressure);
            } else if (_sensor_baro.sensors[i].type == BARO_TYPE_WATER) {
                //101325Pa is sea level air pressure, 9800 Pascal/ m depth in water.
                //No temperature or depth compensation for density of water.
                altitude = (_sensor_baro.sensors[i].ground_pressure - corrected_pressure) / 9800.0f / _sensor_baro._specific_gravity;
                altitude_abs = altitude;
            }
            // sanity check altitude
            _sensor_baro.sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
            if (_sensor_baro.sensors[i].alt_ok) {
                _sensor_baro.sensors[i].altitude = altitude + _sensor_baro._alt_offset_active;
                _sensor_baro.sensors[i].altitude_abs = altitude_abs;
            }
        }
    }

    // ensure the climb rate filter is updated
    if (sensor_baro_healthy2(_sensor_baro._primary)) {
        derivative_filter7_update(&_sensor_baro._climb_rate_filter, sensor_baro_get_altitude(), sensor_baro_get_last_update2(_sensor_baro._primary));
    }

    // choose primary sensor
    if (_sensor_baro._primary_baro >= 0 && _sensor_baro._primary_baro < _sensor_baro._num_sensors && sensor_baro_healthy2(_sensor_baro._primary_baro)) {
        _sensor_baro._primary = _sensor_baro._primary_baro;
    } else {
        _sensor_baro._primary = 0;
        for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
            if (sensor_baro_healthy2(i)) {
                _sensor_baro._primary = i;
                break;
            }
        }
    }

    sensor_baro_publish_baro();
}

static void sensor_baro_publish_baro(void)
{
    uitc_sensor_baro baro;

    baro.timestamp_us = _sensor_baro.sensors[_sensor_baro._primary].last_change_ms*1000;
    baro.primary = _sensor_baro._primary;
    baro.num_instances = _sensor_baro._num_sensors;
    baro.altitude_m = sensor_baro_get_altitude_abs();
    baro.velocity_ms = sensor_baro_get_climb_rate();

    itc_publish(ITC_ID(sensor_baro), &baro);
}

/*
  call accumulate on all drivers
 */
void sensor_baro_accumulate(void)
{
    for (uint8_t i=0; i<_sensor_baro._num_drivers; i++) {
        sensor_baro_backend_accumulate(_sensor_baro.drivers[i]);
    }
}


/* register a new sensor, claiming a sensor slot. If we are out of
   slots it will panic
*/
uint8_t sensor_baro_register_sensor(void)
{
    if (_sensor_baro._num_sensors >= BARO_MAX_INSTANCES) {
        console_panic("Too many barometers");
    }
    return _sensor_baro._num_sensors++;
}


/*
  check if all barometers are healthy
 */
bool sensor_baro_all_healthy(void)
{
     for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
         if (!sensor_baro_healthy2(i)) {
             return false;
         }
     }
     return _sensor_baro._num_sensors > 0;
}

// set a pressure correction from AP_TempCalibration
void sensor_baro_set_pressure_correction(uint8_t instance, float p_correction)
{
    if (instance < _sensor_baro._num_sensors) {
        _sensor_baro.sensors[instance].p_correction = p_correction;
    }
}

// return current climb_rate estimate relative to time that calibrate()
// was called. Returns climb rate in meters/s, positive means up
// note that this relies on read() being called regularly to get new data
float sensor_baro_get_climb_rate(void)
{
    // we use a 7 point derivative filter on the climb rate. This seems
    // to produce somewhat reasonable results on real hardware
    return derivative_filter7_slope(&_sensor_baro._climb_rate_filter) * 1.0e3f;
}

// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
float sensor_baro_get_EAS2TAS(void)
{
    float altitude = sensor_baro_get_altitude();
    if ((fabsf(altitude - _sensor_baro._last_altitude_EAS2TAS) < 25.0f) && !math_flt_zero(_sensor_baro._EAS2TAS)) {
        // not enough change to require re-calculating
        return _sensor_baro._EAS2TAS;
    }

    float pressure = sensor_baro_get_pressure();
    if (math_flt_zero(pressure)) {
        return 1.0f;
    }

    // only estimate lapse rate for the difference from the ground location
    // provides a more consistent reading then trying to estimate a complete
    // ISA model atmosphere
    float tempK = sensor_baro_get_ground_temperature() + C_TO_KELVIN - ISA_LAPSE_RATE * altitude;
    const float eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK));
    if (!math_flt_positive(eas2tas_squared)) {
        return 1.0f;
    }
    _sensor_baro._EAS2TAS = sqrtf(eas2tas_squared);
    _sensor_baro._last_altitude_EAS2TAS = altitude;
    return _sensor_baro._EAS2TAS;
}

// return air density / sea level density - decreases as altitude climbs
float sensor_baro_get_air_density_ratio(void)
{
    const float eas2tas = sensor_baro_get_EAS2TAS();
    if (eas2tas > 0.0f) {
        return 1.0f/(sq(eas2tas));
    } else {
        return 1.0f;
    }
}

// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
void sensor_baro_calibrate(bool save)
{
    // start by assuming all sensors are calibrated (for healthy() test)
    for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
        _sensor_baro.sensors[i].calibrated = true;
        _sensor_baro.sensors[i].alt_ok = true;
    }

    //if (hal.util->was_watchdog_reset()) {
    //    BARO_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: skipping calibration after WDG reset");
    //    return;
    //}

    #ifdef HAL_BARO_ALLOW_INIT_NO_BARO
    if (_sensor_baro._num_drivers == 0 || _sensor_baro._num_sensors == 0 || _sensor_baro.drivers[0] == NULL) {
            SENSOR_BARO_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Baro: no sensors found, skipping calibration");
            return;
    }
    #endif
    
    SENSOR_BARO_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Calibrating barometer");

    // reset the altitude offset when we calibrate. The altitude
    // offset is supposed to be for within a flight
    param_set_and_save(PARAM_ID(BARO, BARO_ALT_OFFSET), 0.0f);

    // let the barometer settle for a full second after startup
    // the MS5611 reads quite a long way off for the first second,
    // leading to about 1m of error if we don't wait
    for (uint8_t i = 0; i < 10; i++) {
        uint32_t tstart = time_millis();
        do {
            sensor_baro_update();
            if (time_millis() - tstart > 500) {
                brd_config_error("Baro: unable to calibrate");
            }
            rt_thread_mdelay(10);
        } while (!sensor_baro_healthy());
        rt_thread_mdelay(100);
    }

    // now average over 5 values for the ground pressure settings
    float sum_pressure[BARO_MAX_INSTANCES] = {0};
    uint8_t count[BARO_MAX_INSTANCES] = {0};
    const uint8_t num_samples = 5;

    for (uint8_t c = 0; c < num_samples; c++) {
        uint32_t tstart = time_millis();
        do {
            sensor_baro_update();
            if (time_millis() - tstart > 500) {
                brd_config_error("Baro: unable to calibrate");
            }
        } while (!sensor_baro_healthy());
        for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
            if (sensor_baro_healthy2(i)) {
                sum_pressure[i] += _sensor_baro.sensors[i].pressure;
                count[i] += 1;
            }
        }
        rt_thread_mdelay(100);
    }
    for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
        if (count[i] == 0) {
            _sensor_baro.sensors[i].calibrated = false;
        } else {
            if (save) {
                sensor_baro_ground_pressure_set_and_save(i, sum_pressure[i] / count[i]);
            }
        }
    }

    _sensor_baro._guessed_ground_temperature = sensor_baro_get_external_temperature();

    // panic if all sensors are not calibrated
    uint8_t num_calibrated = 0;
    for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
        if (_sensor_baro.sensors[i].calibrated) {
            SENSOR_BARO_GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Barometer %u calibration complete", i+1);
            num_calibrated++;
        }
    }
    if (num_calibrated) {
        return;
    }
    brd_config_error("Baro: all sensors uncalibrated");
}

/*
   update the barometer calibration
   this updates the baro ground calibration to the current values. It
   can be used before arming to keep the baro well calibrated
*/
void sensor_baro_update_calibration()
{
    const uint32_t now = time_millis();
    const bool do_notify = now - _sensor_baro._last_notify_ms > 10000;
    if (do_notify) {
        _sensor_baro._last_notify_ms = now;
    }
    for (uint8_t i=0; i<_sensor_baro._num_sensors; i++) {
        if (sensor_baro_healthy2(i)) {
            float corrected_pressure = sensor_baro_get_pressure2(i) + _sensor_baro.sensors[i].p_correction;
            _sensor_baro.sensors[i].ground_pressure = corrected_pressure;
        }

        // don't notify the GCS too rapidly or we flood the link
        if (do_notify) {
            //_sensor_baro.sensors[i].ground_pressure;
        }
    }

    // always update the guessed ground temp
    _sensor_baro._guessed_ground_temperature = sensor_baro_get_external_temperature();

    // force EAS2TAS to recalculate
    _sensor_baro._EAS2TAS = 0;
}

/*
  set external temperature to be used for calibration (degrees C)
 */
void sensor_baro_set_external_temperature(float temperature)
{
    _sensor_baro._external_temperature = temperature;
    _sensor_baro._last_external_temperature_ms = time_millis();
}

/*
  get the temperature in degrees C to be used for calibration purposes
 */
float sensor_baro_get_external_temperature2(const uint8_t instance)
{
    // if we have a recent external temperature then use it
    if (_sensor_baro._last_external_temperature_ms != 0 && time_millis() - _sensor_baro._last_external_temperature_ms < 10000) {
        return _sensor_baro._external_temperature;
    }

#if 0
#ifndef HAL_BUILD_AP_PERIPH
    // if we don't have an external temperature then try to use temperature
    // from the airspeed sensor
    AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
    if (airspeed != nullptr) {
        float temperature;
        if (airspeed->healthy() && airspeed->get_temperature(temperature)) {
            return temperature;
        }
    }
#endif
#endif

    // if we don't have an external temperature and airspeed temperature
    // then use the minimum of the barometer temperature and 35 degrees C.
    // The reason for not just using the baro temperature is it tends to read high,
    // often 30 degrees above the actual temperature. That means the
    // EAS2TAS tends to be off by quite a large margin, as well as
    // the calculation of altitude difference betweeen two pressures
    // reporting a high temperature will cause the aircraft to
    // estimate itself as flying higher then it actually is.
    return MIN(sensor_baro_get_temperature2(instance), INTERNAL_TEMPERATURE_CLAMP);
}

// Set the type (Air or Water) of a particular instance
void sensor_baro_set_type(uint8_t instance, baro_type_t type) { _sensor_baro.sensors[instance].type = type; };

// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool sensor_baro_arming_checks(size_t buflen, char *buffer)
{
    if (!sensor_baro_all_healthy()) {
        snprintf(buffer, buflen, "not healthy");
        return false;
    }

#if 0
#if MB_BUILD_COPTER_OR_HELI || MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
    /*
      check for a pressure altitude discrepancy between GPS alt and
      baro alt this catches bad barometers, such as when a MS5607 has
      been substituted for a MS5611
     */
    const auto &gps = AP::gps();
    if (_alt_error_max > 0 && gps.status() >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
        const float alt_amsl = gps.location().alt*0.01;
        const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure());
        const float error = fabsf(alt_amsl - alt_pressure);
        if (error > _alt_error_max) {
            hal.util->snprintf(buffer, buflen, "GPS alt error %.0fm (see BARO_ALTERR_MAX)", error);
            return false;
        }
    }
#endif
#endif

    return true;
}

static bool _add_backend(sensor_baro_backend *backend)
{
    if (!backend) {
        return false;
    }
    if (_sensor_baro._num_drivers >= BARO_MAX_DRIVERS) {
        console_panic("Too many barometer drivers");
    }
    _sensor_baro.drivers[_sensor_baro._num_drivers++] = backend;
    return true;
}

/*
  probe all the i2c barometers enabled with BARO_PROBE_EXT. This is
  used on boards without a builtin barometer
 */
static void _probe_i2c_barometers(void)
{
    uint32_t probe = _sensor_baro._baro_probe_ext;
    (void)probe;  // may be unused if most baros compiled out
    uint32_t mask = devmgr_get_bus_mask_external();
    if (brd_get_board_type() == PX4_BOARD_PIXHAWK2) {
        // for the purpose of baro probing, treat CubeBlack internal i2c as external. It has
        // no internal i2c baros, so this is safe
        mask |= devmgr_get_bus_mask_internal();
    }

    // if the user has set GND_EXT_BUS then probe the bus given by that parameter
    int8_t ext_bus = _sensor_baro._ext_bus;
    if (ext_bus >= 0) {
        mask = 1U << (uint8_t)ext_bus;
    }

    static const struct BaroProbeSpec {
        uint32_t bit;
        sensor_baro_backend* (*probefn)(gp_device_t dev);
        uint8_t addr;
    } baroprobespec[] = {
        { PROBE_SPL06, sensor_baro_spl06_probe, HAL_BARO_SPL06_I2C_ADDR },
        { PROBE_SPL06, sensor_baro_spl06_probe, HAL_BARO_SPL06_I2C_ADDR2 },
    };

    for (uint8_t j=0; j<ARRAY_SIZE(baroprobespec); j++) {
        if (!(probe & baroprobespec[j].bit)) {
            // not in mask to be probed for
            continue;
        }
        FOREACH_I2C_MASK(i, mask) {
            ADD_BACKEND(baroprobespec[j].probefn(GET_I2C_DEVICE(i, baroprobespec[j].addr)));
        }
    }
}

/*------------------------------------test------------------------------------*/


